//
//  THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY
//  KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
//  IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR
//  PURPOSE. IT CAN BE DISTRIBUTED FREE OF CHARGE AS LONG AS THIS HEADER 
//  REMAINS UNCHANGED.
//
//  Email:  gustavo_franco@hotmail.com
//
//  Copyright (C) 2006 Franco, Gustavo 
//
//  Modified by Alexandr Levin (a.l. - marked places)
//
//#define DEBUGON
#define DEBUGTIMER

using System;
using System.Collections.Generic;
using System.Runtime.InteropServices;

namespace AntBot.Algorithms
{
	#region Structs
	public struct PathFinderNode
	{
		#region Variables Declaration
		public int F;
		public int G;
		public int H;  // f = gone + heuristic
		public int X;
		public int Y;
		public int PX; // Parent
		public int PY;
		#endregion
	}
	#endregion

	#region Enum
	public enum HeuristicFormula
	{
		Manhattan = 1,
		MaxDXDY = 2,
		DiagonalShortCut = 3,
		Euclidean = 4,
		EuclideanNoSQR = 5,
		Custom1 = 6
	}
	#endregion

	public class PathFinderFast : IPathFinder
	{
		#region Structs
		[StructLayout(LayoutKind.Sequential, Pack = 1)]
		internal struct PathFinderNodeFast
		{
			#region Variables Declaration
			public int F; // f = gone + heuristic
			public int G;
			public ushort PX; // Parent
			public ushort PY;
			public byte Status;
			#endregion
		}
		#endregion

		#region Variables Declaration
		// Heap variables are initializated to default, but I like to do it anyway
		private readonly byte[,] mGrid;
		private readonly PriorityQueueB<int> mOpen;
		private readonly List<PathFinderNode> mClose = new List<PathFinderNode>();
		private bool mStop;
		private bool mStopped = true;
		private int mHoriz;
		private HeuristicFormula mFormula = HeuristicFormula.Manhattan;
		private bool mDiagonals = true;
		private int mHEstimate = 2;
		private bool mPunishChangeDirection;
		private bool mTieBreaker;
		private bool mHeavyDiagonals;
		private int mSearchLimit = 2000;
		private double mCompletedTime;
		private bool mDebugProgress;
		private bool mDebugFoundPath;
		private readonly PathFinderNodeFast[] mCalcGrid;
		private byte mOpenNodeValue = 1;
		private byte mCloseNodeValue = 2;

		//Promoted local variables to member variables to avoid recreation between calls
		private int mH;
		private int mLocation;
		private int mNewLocation;
		private ushort mLocationX;
		private ushort mLocationY;
		private ushort mNewLocationX;
		private ushort mNewLocationY;
		private int mCloseNodeCounter;
		private readonly ushort mGridX;
		private readonly ushort mGridY;
		private readonly ushort mGridRealX;
		private readonly ushort mGridRealY;
		private readonly ushort mGridXMinus1;
		private readonly ushort mGridXLog2;
		private bool mFound;
		private sbyte[,] mDirection = new sbyte[8, 2] { { 0, -1 }, { 1, 0 }, { 0, 1 }, { -1, 0 }, { 1, -1 }, { 1, 1 }, { -1, 1 }, { -1, -1 } };
		private int mEndLocation;
		private int mNewG;
		private readonly Timer mTimer;
		#endregion

		#region Constructors
		public PathFinderFast(byte[,] grid)
		{
			if (grid == null)
				throw new Exception("Grid cannot be null");

			mTimer = new Timer();

			SetDefaultSettings();

			//a.l. Permit to work with any grid sizes (not only pow of 2)
			mGridRealX = (ushort)(grid.GetUpperBound(0) + 1);
			mGridRealY = (ushort)(grid.GetUpperBound(1) + 1);

			mGridX = FindClosestPowOf2(mGridRealX);
			mGridY = FindClosestPowOf2(mGridRealY);

			if (mGridRealX != mGridX || mGridRealY != mGridY)
			{
				mGrid = new byte[mGridX, mGridY];
				//fill for mGridRealX and mGridRealY. Rest will be 0 (wall)
				for (int x = 0; x < mGridRealX; x++)
					for (int y = 0; y < mGridRealY; y++)
						mGrid[x, y] = grid[x, y];
			}
			else
			{
				mGrid = grid;
			}

			mGridXMinus1 = (ushort)(mGridX - 1);
			mGridXLog2 = (ushort)Math.Log(mGridX, 2);

			if (mCalcGrid == null || mCalcGrid.Length != (mGridX * mGridY))
				mCalcGrid = new PathFinderNodeFast[mGridX * mGridY];

			mOpen = new PriorityQueueB<int>(new ComparePFNodeMatrix(mCalcGrid));
		}
		#endregion

		#region Properties

		//a.l. Shortest path
		public bool IsShortest { get; private set; }

		public bool Stopped
		{
			get { return mStopped; }
		}

		public HeuristicFormula Formula
		{
			get { return mFormula; }
			set { mFormula = value; }
		}

		public bool Diagonals
		{
			get { return mDiagonals; }
			set
			{
				mDiagonals = value;
// ReSharper disable ConvertIfStatementToConditionalTernaryExpression
				if (mDiagonals)
// ReSharper restore ConvertIfStatementToConditionalTernaryExpression
					mDirection = new sbyte[8, 2] { { 0, -1 }, { 1, 0 }, { 0, 1 }, { -1, 0 }, { 1, -1 }, { 1, 1 }, { -1, 1 }, { -1, -1 } };
				else
					mDirection = new sbyte[4, 2] { { 0, -1 }, { 1, 0 }, { 0, 1 }, { -1, 0 } };
			}
		}

		public bool HeavyDiagonals
		{
			get { return mHeavyDiagonals; }
			set { mHeavyDiagonals = value; }
		}

		public int HeuristicEstimate
		{
			get { return mHEstimate; }
			set { mHEstimate = value; }
		}

		public bool PunishChangeDirection
		{
			get { return mPunishChangeDirection; }
			set { mPunishChangeDirection = value; }
		}

		public bool TieBreaker
		{
			get { return mTieBreaker; }
			set { mTieBreaker = value; }
		}

		public int SearchLimit
		{
			get { return mSearchLimit; }
			set { mSearchLimit = value; }
		}

		public double CompletedTime
		{
			get { return mCompletedTime; }
			set { mCompletedTime = value; }
		}

		public bool DebugProgress
		{
			get { return mDebugProgress; }
			set { mDebugProgress = value; }
		}

		public bool DebugFoundPath
		{
			get { return mDebugFoundPath; }
			set { mDebugFoundPath = value; }
		}
		#endregion

		#region Methods
		public List<PathFinderNode> FindPath(Location start, Location end)
		{
			lock (this)
			{
				#if DEBUGTIMER
				mTimer.Start();
				#endif

				// Is faster if we don't clear the matrix, just assign different values for open and close and ignore the rest

				mFound = false;
				mStop = false;
				mStopped = false;
				mCloseNodeCounter = 0;
				mOpenNodeValue += 2;
				mCloseNodeValue += 2;
				mOpen.Clear();
				mClose.Clear();

				mLocation = (start.Y << mGridXLog2) + start.X;
				mEndLocation = (end.Y << mGridXLog2) + end.X;
				mCalcGrid[mLocation].G = 0;
				mCalcGrid[mLocation].F = mHEstimate;
				mCalcGrid[mLocation].PX = (ushort)start.X;
				mCalcGrid[mLocation].PY = (ushort)start.Y;
				mCalcGrid[mLocation].Status = mOpenNodeValue;

				mOpen.Push(mLocation);
				while (mOpen.Count > 0 && !mStop)
				{
					mLocation = mOpen.Pop();

					//Is it in closed list? means this node was already processed
					if (mCalcGrid[mLocation].Status == mCloseNodeValue)
						continue;

					mLocationX = (ushort)(mLocation & mGridXMinus1);
					mLocationY = (ushort)(mLocation >> mGridXLog2);

					if (mLocation == mEndLocation)
					{
						mCalcGrid[mLocation].Status = mCloseNodeValue;
						mFound = true;
						break;
					}

					if (mCloseNodeCounter > mSearchLimit)
					{
						mStopped = true;
						#if DEBUGTIMER
						mCompletedTime = mTimer.GetTime();
						#endif
						return null;
					}

					if (mPunishChangeDirection)
						mHoriz = (mLocationX - mCalcGrid[mLocation].PX);

					//Lets calculate each successors
					for (int i = 0; i < (mDiagonals ? 8 : 4); i++)
					{
						//a.l. comment Wrapped Edges
						/*mNewLocationX = (ushort) (mLocationX + mDirection[i,0]);
						mNewLocationY = (ushort) (mLocationY + mDirection[i,1]);
						mNewLocation = (mNewLocationY << mGridYLog2) + mNewLocationX;

						if (mNewLocationX >= mGridX || mNewLocationY >= mGridY)
							//continue;*/

						//a.l. Wrapped edges
						int newLocX = (mLocationX + mDirection[i, 0]);
						int newLocY = (mLocationY + mDirection[i, 1]);

						if (newLocX >= mGridRealX) newLocX -= mGridRealX;
						if (newLocX < 0) newLocX +=  mGridRealX;

						if (newLocY >= mGridRealY) newLocY -= mGridRealY;
						if (newLocY < 0) newLocY += mGridRealY;

						mNewLocationX = (ushort)newLocX;
						mNewLocationY = (ushort)newLocY;
						mNewLocation = (mNewLocationY << mGridXLog2) + mNewLocationX;

						// Unbreakeable?
						if (mGrid[mNewLocationX, mNewLocationY] == 0)
							continue;

						if (mHeavyDiagonals && i > 3)
							mNewG = mCalcGrid[mLocation].G + (int)(mGrid[mNewLocationX, mNewLocationY] * 2.41);
						else//								//a.l. grid[x,y] > 100 means unknown square (suppose it's land)
							mNewG = mCalcGrid[mLocation].G + mGrid[mNewLocationX, mNewLocationY] <= 100 ? 
																mGrid[mNewLocationX, mNewLocationY] : 
																mGrid[mNewLocationX, mNewLocationY] - 100;

						if (mPunishChangeDirection)
						{
							if ((mNewLocationX - mLocationX) != 0)
							{
								if (mHoriz == 0)
									mNewG += Math.Abs(mNewLocationX - end.X) + Math.Abs(mNewLocationY - end.Y);
							}
							if ((mNewLocationY - mLocationY) != 0)
							{
								if (mHoriz != 0)
									mNewG += Math.Abs(mNewLocationX - end.X) + Math.Abs(mNewLocationY - end.Y);
							}
						}

						//Is it open or closed?
						if (mCalcGrid[mNewLocation].Status == mOpenNodeValue || mCalcGrid[mNewLocation].Status == mCloseNodeValue)
						{
							// The current node has less code than the previous? then skip this node
							if (mCalcGrid[mNewLocation].G <= mNewG)
								continue;
						}

						mCalcGrid[mNewLocation].PX = mLocationX;
						mCalcGrid[mNewLocation].PY = mLocationY;
						mCalcGrid[mNewLocation].G = mNewG;

						switch (mFormula)
						{
							default:
// ReSharper disable RedundantCaseLabel
							case HeuristicFormula.Manhattan:
// ReSharper restore RedundantCaseLabel
								//a.l.
								mH = mHEstimate * Torus.ManhattanDistance(new Location(mNewLocationX, mNewLocationY), end, mGridRealX, mGridRealY);
								//mH = mHEstimate * (Math.Abs(mNewLocationX - end.X) + Math.Abs(mNewLocationY - end.Y));
								break;
							case HeuristicFormula.MaxDXDY:
								//a.l.
								mH = mHEstimate * (Math.Max(Torus.DV(mNewLocationX, end.X, mGridRealX), Torus.DV(mNewLocationY, end.Y, mGridRealY)));
								//mH = mHEstimate * (Math.Max(Math.Abs(mNewLocationX - end.X), Math.Abs(mNewLocationY - end.Y)));
								break;
							case HeuristicFormula.DiagonalShortCut:
								//a.l.
								int hDiagonal = Math.Min(Torus.DV(mNewLocationX, end.X, mGridRealX), Torus.DV(mNewLocationY, end.Y, mGridRealY));
								int hStraight = Torus.DV(mNewLocationX, end.X, mGridRealX) + Torus.DV(mNewLocationY, end.Y, mGridRealY);
								mH = (int)((mHEstimate + 0.41) * hDiagonal + mHEstimate * (hStraight - 2 * hDiagonal));
								break;
							case HeuristicFormula.Euclidean:
								//a.l. Wrapped Edges
								mH = (int)(mHEstimate * Math.Sqrt(Torus.Distance2(mNewLocationX, mNewLocationY, end.X, end.Y, mGridRealX, mGridRealY)));
								//mH = (int)(mHEstimate * Math.Sqrt(Math.Pow((mNewLocationY - end.X), 2) + Math.Pow((mNewLocationY - end.Y), 2)));
								break;
							case HeuristicFormula.EuclideanNoSQR:
								//a.l. Wrapped Edges
								mH = (mHEstimate * Torus.Distance2(mNewLocationX, mNewLocationY, end.X, end.Y, mGridRealX, mGridRealY));
								//mH = (int)(mHEstimate * (Math.Pow((mNewLocationX - end.X), 2) + Math.Pow((mNewLocationY - end.Y), 2)));
								break;
							case HeuristicFormula.Custom1:
								//a.l.
								Location dxy = new Location(Torus.DV(mNewLocationX, end.X, mGridRealX), Torus.DV(mNewLocationY, end.Y, mGridRealY));
								//Location dxy = new Location(Math.Abs(end.X - mNewLocationX), Math.Abs(end.Y - mNewLocationY));
								int orthogonal = Math.Abs(dxy.X - dxy.Y);
								int diagonal = Math.Abs(((dxy.X + dxy.Y) - orthogonal) / 2);
								mH = mHEstimate * (diagonal + orthogonal + dxy.X + dxy.Y);
								break;
						}
						if (mTieBreaker)
						{
							int dx1 = mLocationX - end.X;
							int dy1 = mLocationY - end.Y;
							int dx2 = start.X - end.X;
							int dy2 = start.Y - end.Y;
							int cross = Math.Abs(dx1 * dy2 - dx2 * dy1);
							mH = (int)(mH + cross * 0.001);
							//a.l. for equal pathes always chose without unknown squares
							if (mGrid[mNewLocationX, mNewLocationY] > 100) mH += 10;
						}
						mCalcGrid[mNewLocation].F = mNewG + mH;

#if DEBUGON
                        if (mDebugProgress && PathFinderDebug != null)
                            PathFinderDebug(mLocationX, mLocationY, mNewLocationX, mNewLocationY, PathFinderNodeType.Open, mCalcGrid[mNewLocation].F, mCalcGrid[mNewLocation].G);
#endif

						mOpen.Push(mNewLocation);
						mCalcGrid[mNewLocation].Status = mOpenNodeValue;
					}

					mCloseNodeCounter++;
					mCalcGrid[mLocation].Status = mCloseNodeValue;

#if DEBUGON
                    if (mDebugProgress && PathFinderDebug != null)
                        PathFinderDebug(0, 0, mLocationX, mLocationY, PathFinderNodeType.Close, mCalcGrid[mLocation].F, mCalcGrid[mLocation].G);
#endif
				}

				#if DEBUGTIMER
				mCompletedTime = mTimer.GetTime();
				#endif

				if (mFound)
				{
					mClose.Clear();
// ReSharper disable TooWideLocalVariableScope
					int posX;
					int posY;
// ReSharper restore TooWideLocalVariableScope

					//a.l. Shortest path
					IsShortest = true;

					PathFinderNodeFast fNodeTmp = mCalcGrid[(end.Y << mGridXLog2) + end.X];
					PathFinderNode fNode;
					fNode.F = fNodeTmp.F;
					fNode.G = fNodeTmp.G;
					fNode.H = 0;
					fNode.PX = fNodeTmp.PX;
					fNode.PY = fNodeTmp.PY;
					fNode.X = end.X;
					fNode.Y = end.Y;

					while (fNode.X != fNode.PX || fNode.Y != fNode.PY)
					{
						mClose.Add(fNode);
#if DEBUGON
                        if (mDebugFoundPath && PathFinderDebug != null)
                            PathFinderDebug(fNode.PX, fNode.PY, fNode.X, fNode.Y, PathFinderNodeType.Path, fNode.F, fNode.G);
#endif
						posX = fNode.PX;
						posY = fNode.PY;
						fNodeTmp = mCalcGrid[(posY << mGridXLog2) + posX];
						fNode.F = fNodeTmp.F;
						fNode.G = fNodeTmp.G;
						fNode.H = 0;
						fNode.PX = fNodeTmp.PX;
						fNode.PY = fNodeTmp.PY;
						fNode.X = posX;
						fNode.Y = posY;

						//a.l. ShortestPath
						if (IsShortest && mGrid[posX, posY] > 100)
							IsShortest = false;
					}

					mClose.Add(fNode);
#if DEBUGON
                    if (mDebugFoundPath && PathFinderDebug != null)
                        PathFinderDebug(fNode.PX, fNode.PY, fNode.X, fNode.Y, PathFinderNodeType.Path, fNode.F, fNode.G);
#endif

					mStopped = true;
					return mClose;
				}
				mStopped = true;
				return null;
			}
		}

		public void SetDefaultSettings()
		{
			Formula = Config.A_STAR_HEURISTIC;
			Diagonals = Config.A_STAR_DIAGONALS;
			HeavyDiagonals = Config.A_STAR_HEAVY_DIAGONALS;
			HeuristicEstimate = Config.A_STAR_HEURISTIC_ESTIMATE;
			PunishChangeDirection = Config.A_STAR_PUNISH_CHANGE_DIRECTION;
			TieBreaker = Config.A_STAR_TIE_BREAKER;
		}

		public void FindPathStop()
		{
			mStop = true;
		}

		public static ushort FindClosestPowOf2(ushort x)
		{
			ushort pow = 1;
			while (pow < x) pow *= 2;
			return pow;
		}

		public bool ChangeSquareValue(ushort x, ushort y, byte newValue)
		{
			if (mGrid == null) return false;
			if (x >= mGridRealX) return false;
			if (y >= mGridRealY) return false;
			mGrid[x, y] = newValue;
			return true;
		}
		#endregion

		#region Inner Classes
		internal class ComparePFNodeMatrix : IComparer<int>
		{
			#region Variables Declaration

			readonly PathFinderNodeFast[] mMatrix;
			#endregion

			#region Constructors
			public ComparePFNodeMatrix(PathFinderNodeFast[] matrix)
			{
				mMatrix = matrix;
			}
			#endregion

			#region IComparer Members
			public int Compare(int a, int b)
			{
				if (mMatrix[a].F > mMatrix[b].F)
					return 1;
				if (mMatrix[a].F < mMatrix[b].F)
					return -1;
				return 0;
			}
			#endregion
		}
		#endregion
	}
}